
#2020-07-29
#by eric
#

import sys
import pinocchio as pin
from pinocchio.visualize import MeshcatVisualizer
import time

URDF = '../../model/double_joint/urdf/double_joint.urdf'

model, collision_model, visual_model = pin.buildModelsFromUrdf(URDF)

viz = MeshcatVisualizer(model, collision_model, visual_model)


try:
    viz.initViewer(open=True)
except ImportError as err:
    print("Error while initializing the viewer. It seems you should install Python meshcat")
    print(err)
    sys.exit(0)

# Load the robot in the viewer.
viz.loadViewerModel()
# Display a robot configuration.
q0 = pin.neutral(model)
q = q0
q[0] = q[0] +0.2
viz.display(q)

time.sleep(10)

